#include "App_Patrol.h"
#include "Int_Patrol.h"
#include "Int_Motor.h"
#include "Int_OLED_Data.h"

// 定义PID系数
#define KI 1      // 积分系数
#define KD 600    // 微分系数
#define KP 1000 // 比例系数

// 错误总积分
static int s_sum_error = 0;
// 上一次的误差
static int s_last_error = 0;

/**
 * @brief 小车巡逻控制
 *
 */
void App_Patrol_Controller()
{
    int result;
    // 获取小车偏差值,
    char error_weight;
    error_weight = Int_Patrol_Counter();

    s_sum_error = error_weight + s_sum_error;
    if (s_sum_error >= 3000) {
        s_sum_error = 3000;
    }
    if (s_sum_error <= -3000) {
        s_sum_error = -3000;
    }

    // 微分：当前误差减去上次误差
    // 计算结果：  误差*比例系数 + 累加误差*积分系数 + (当前误差-上次误差)*微分系数
    result       = error_weight * KP + s_sum_error * KI + (error_weight - s_last_error) * KD;
    s_last_error = error_weight;

    // 处理结果
    result /= 50;
    if (result > 80) {
        result = 80;
    } else if (result < -80) {
        result = -80;
    }
    Int_Motor_setRight(40 - result);
    Int_Motor_setLeft(40 + result);
}